﻿using System;
using System.Collections.Generic;
using System.Windows.Forms;
using System.IO;
using GeoFits;

namespace GeoFitsTest
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void btn_circleFit_Click(object sender, EventArgs e)
        {
            string filename = "circlefit1.txt";
            if (!File.Exists(filename))
            {
                MessageBox.Show("请将circle文件放到与执行文件相同的目录中后再试！");
                return;
            }
            List<Point> pts = ReadPtsFromFile(filename);
            //二维圆心拟合
            var c4Result = CircleFit.Fit2DInFastMode(pts);
            Circle c4 = c4Result.ResultGeometry as Circle;
            tb_info.Text += "快速拟合圆心坐标为：" + c4.Center.X.ToString("F4") + "," + c4.Center.Y.ToString("F4") + "," + c4.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + c4.Radius.ToString("F4") + "\r\n";
            tb_info.Text += "偏差V：";
            for(int i = 0; i < c4Result.V.NoRows; i++)
            {
                tb_info.Text += c4Result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += c4.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + c4Result.sigma0.ToString("F4") + "\r\n";

            c4Result = CircleFit.Fit2D(pts);
            c4 = c4Result.ResultGeometry as Circle;
            tb_info.Text += "高精度圆心坐标为：" + c4.Center.X.ToString("F4") + "," + c4.Center.Y.ToString("F4") + "," + c4.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + c4.Radius.ToString("F4") + "\r\n";

            tb_info.Text += "偏差V：";
            for (int i = 0; i < c4Result.V.NoRows; i++)
            {
                tb_info.Text += c4Result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += c4.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + c4Result.sigma0.ToString("F4") + "\r\n";
        }

        private static List<Point> ReadPtsFromFile(string fileName)
        {
            StreamReader sr = new StreamReader(fileName);
            List<Point> pts = new List<Point>();
            do
            {
                string line = sr.ReadLine();
                if (line == null)
                {
                    break;
                }
                string[] xyz = line.Split(',');
                Point tempt;
                if (xyz.Length < 3)
                {
                    tempt = new Point(Convert.ToDouble(xyz[0]), Convert.ToDouble(xyz[1]), 0);
                }else
                {
                    if(xyz[2].Trim()=="")                    
                        tempt = new Point(Convert.ToDouble(xyz[0]), Convert.ToDouble(xyz[1]), 0);
                    else
                        tempt = new Point(Convert.ToDouble(xyz[0]), Convert.ToDouble(xyz[1]), Convert.ToDouble(xyz[2]));
                }
                
                pts.Add(tempt);
            } while (true);
            return pts;
        }

        private static void SavePts2File(List<Point> pts, string fileName)
        {
            StreamWriter sw = new StreamWriter(fileName, false);
            foreach(var pt in pts)
            {
                sw.WriteLine(pt.X.ToString("F4") + "," + pt.Y.ToString("F4") + "," + pt.Z.ToString("F4"));
            }
            sw.Flush();
            sw.Close();
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            Quaternion qt1 = new Quaternion(1, 2, 2, 1);
            Quaternion qt2 = new Quaternion(0, 1, 5, 6);
            Quaternion qt3 = qt1 * qt2 * qt1.Inverse();
            var qt5 = qt1.Normalize();
            Quaternion qt4 = qt5 * qt2 * qt5.Inverse();
            //List<GeoFits.Geometry> abclist = null;
            //abclist = new List<GeoFits.Geometry>();

            //if(object.Equals(abclist,null))
            //{
            //    int i = 0;
            //}
            //if(ReferenceEquals(abclist,null))
            //{
            //    int m = 0;
            //}
        }

        private void btn_pfit_Click(object sender, EventArgs e)
        {
            List<Point> pts = ReadPtsFromFile("planefit.txt");
            Plane plane = PlaneFit.Fit(pts).ResultGeometry as Plane;
            tb_info.Text += "Point On Plane:" + plane.PointOnPlane.X.ToString("F4") + "," +
                plane.PointOnPlane.Y.ToString("F4") + "," +
                plane.PointOnPlane.Z.ToString("F4") + "\r\n";
            tb_info.Text += "Normal:" + plane.Normal.X.ToString("F4") + "," +
                plane.Normal.Y.ToString("F4") + "," +
                plane.Normal.Z.ToString("F4") + "\r\n";
            tb_info.Text += "Diff:";
            double allDist = 0;
            foreach (var pt in pts)
            {
                double dist = plane.DistanceTo(pt);
                allDist += Math.Abs(dist);
                tb_info.Text += dist.ToString("F5") + ",";
            }
            tb_info.Text += "\r\nAll Diff:" + allDist.ToString("F15") + "\r\n";
        }

        private void btn_lfit_Click(object sender, EventArgs e)
        {
            List<Point> pts = ReadPtsFromFile("linefit.txt");
            Line result = LineFit.Fit(pts, true).ResultGeometry as Line;
            tb_info.Text += "Point On Line:" + result.PointOnLine.X.ToString("F4") + "," +
                result.PointOnLine.Y.ToString("F4") + "," +
                result.PointOnLine.Z.ToString("F4") + "\r\n";
            tb_info.Text += "Direction:" + result.Direction.X.ToString("F4") + "," +
                result.Direction.Y.ToString("F4") + "," +
                result.Direction.Z.ToString("F4") + "\r\n";
            tb_info.Text += "Diff:";
            double allDist = 0;
            foreach(var pt in pts)
            {
                allDist += result.DistanceTo(pt);
                tb_info.Text += result.DistanceTo(pt).ToString("F5") + ",";
            }
            tb_info.Text += "\r\nAll Diff:" + allDist.ToString("F15") + "\r\n";
        }

        private void btn_change_Click(object sender, EventArgs e)
        {
            List<Point> pts = ReadPtsFromFile("circlefit2.txt");
            Vector axis = new Vector(2.5, 1.4, -3.1);
            Quaternion qt = Quaternion.getQuaternion(new Vector(0, 0, 1), axis);
            List<Point> newPts = Quaternion.Rotation(pts, qt);
            SavePts2File(newPts, "circlefit3");
        }

        private void btn_3dcfit_Click(object sender, EventArgs e)
        {
            string filename = "circlefit3.txt";
            if (!File.Exists(filename))
            {
                MessageBox.Show("请将circle文件放到与执行文件相同的目录中后再试！");
                return;
            }
            List<Point> pts = ReadPtsFromFile(filename);
            //平面拟合
            var plane = PlaneFit.Fit(pts).ResultGeometry as Plane;
            //旋转
            var qt = Quaternion.getQuaternion(plane.Normal, new Vector(0, 0, 1));
            var newPtList = Quaternion.Rotation(pts, qt);
            //平面圆心拟合
            var result = CircleFit.Fit2D(newPtList);
            Circle circle = result.ResultGeometry as Circle;
            //反向旋转
            circle.Center = Quaternion.Rotation(circle.Center, qt.Inverse());
            circle.Normal = plane.Normal;

            tb_info.Text += "空间拟合圆心坐标为：" + circle.Center.X.ToString("F4") + "," + circle.Center.Y.ToString("F4") + "," + circle.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "空间拟合法向量为：" + circle.Normal.X.ToString("F4") + "," + circle.Normal.Y.ToString("F4") + "," + circle.Normal.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + circle.Radius.ToString("F4") + "\r\n";
            tb_info.Text += "偏差V：";
            for (int i = 0; i < result.V.NoRows; i++)
            {
                tb_info.Text += result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += circle.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + result.sigma0.ToString("F4") + "\r\n";
        }

        private void btn_rlscfit_Click(object sender, EventArgs e)
        {
            string filename = "circlefit1.txt";
            if (!File.Exists(filename))
            {
                MessageBox.Show("请将circle文件放到与执行文件相同的目录中后再试！");
                return;
            }
            List<Point> pts = ReadPtsFromFile(filename);
            RLSConfig config = new RLSConfig();
            config.getWeight = Adjustment.getWeightL1;
            var result = CircleFit.Fit2D(pts, null, 0, config);
            var circle = result.ResultGeometry as Circle;
            tb_info.Text += "L1抗差圆心坐标为：" + circle.Center.X.ToString("F4") + "," + circle.Center.Y.ToString("F4") + "," + circle.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + circle.Radius.ToString("F4") + "\r\n";
            tb_info.Text += "偏差V：";
            for (int i = 0; i < result.V.NoRows; i++)
            {
                tb_info.Text += result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += circle.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + result.sigma0.ToString("F4") + "\r\n";

            RLSConfig config2 = new RLSConfig();
            //config2.setWeightConditions(3, 3);
            result = CircleFit.Fit2D(pts, null, 0, config2);
            circle = result.ResultGeometry as Circle;
            tb_info.Text += "抗差圆心坐标为：" + circle.Center.X.ToString("F4") + "," + circle.Center.Y.ToString("F4") + "," + circle.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + circle.Radius.ToString("F4") + "\r\n";
            tb_info.Text += "偏差V：";
            for (int i = 0; i < result.V.NoRows; i++)
            {
                tb_info.Text += result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += circle.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + result.sigma0.ToString("F4") + "\r\n";

            config2.setWeightConditions(3, 3);
            result = CircleFit.Fit2D(pts, null, 0, config2);
            circle = result.ResultGeometry as Circle;
            tb_info.Text += "抗差圆心坐标为(3S)：" + circle.Center.X.ToString("F4") + "," + circle.Center.Y.ToString("F4") + "," + circle.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + circle.Radius.ToString("F4") + "\r\n";
            tb_info.Text += "偏差V：";
            for (int i = 0; i < result.V.NoRows; i++)
            {
                tb_info.Text += result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += circle.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + result.sigma0.ToString("F4") + "\r\n";
        }

        private void btn_cyfit_Click(object sender, EventArgs e)
        {
            string filename = "cyfit.txt";
            if (!File.Exists(filename))
            {
                MessageBox.Show("请将文件放到与执行文件相同的目录中后再试！");
                return;
            }
            List<Point> pts = ReadPtsFromFile(filename);
            var result = CylinderFit.Fit(pts);
            var cy = result.ResultGeometry as Cylinder;

            tb_info.Text += "圆柱面拟合轴线上一点坐标为：" + cy.Center.X.ToString("F4") + "," + cy.Center.Y.ToString("F4") + "," + cy.Center.Z.ToString("F4") + "\r\n";
            tb_info.Text += "空间拟合法向量为：" + cy.Direct.X.ToString("F4") + "," + cy.Direct.Y.ToString("F4") + "," + cy.Direct.Z.ToString("F4") + "\r\n";
            tb_info.Text += "半径为：" + cy.Radius.ToString("F4") + "\r\n";
            tb_info.Text += "偏差V：";
            for (int i = 0; i < result.V.NoRows; i++)
            {
                tb_info.Text += result.V[i, 0].ToString("F4") + ",";
            }
            tb_info.Text += "\r\n";
            tb_info.Text += "偏差：";
            for (int i = 0; i < pts.Count; i++)
            {
                tb_info.Text += cy.DistanceTo(pts[i]).ToString("F4") + ",";
            }
            tb_info.Text += "\r\n单位权中误差为：" + result.sigma0.ToString("F4") + "\r\n";

        }
    }
}
